#pragma once

#include "zuc/nml_intf/zuc.hh"
#include "libnml/nml/cmd_msg.hh"
#include "libnml/nml/stat_msg.hh"
#include "zuc/nml_intf/zucpos.h"
#include "zuc/motion/safezone.h"
#include "libnml/cms/cms.hh"
#include "zuc_nml_rob_cmd.hh"

DEC_CMD_PN(ZUC_ROBOT_SAFETY, int custome; int lastCustome; int paramType; float momentumLimit; float tcpVelLimit; float robotPowerLimit; float stoppingDistance;
           float stoppingTime;
           int type);

DEC_CMD_PN(ZUC_SET_ROBOT_SAFETY_CUSTOMIZE, int customize_type);

DEC_CMD_PN(ZUC_REDUCE_SAFETY, double tcp_speed; double elbow_speed; double momentum_limit; double power_limit; double stopping_time; double stopping_dist);

DEC_CMD_PN(ZUC_DRAG_SPEED_LIMIT, double drag_tcp_speed_limit);

DEC_CMD_PN(ZUC_JOINT_SAFETY, int jointNum; int paramType;  // 0: joint limit; 1: joint vel limit; 2: joint err limit
           JointPosLimit posLimt;
           float jointVelLimit;
           float jointErrLimit;
           int type;  // 0:set 1:get
);

DEC_CMD_PN(ZUC_FORCE_LIMIT_VALUE, int force_limit_value);

DEC_CMD_PN(ZUC_COLLISION_DETECTION_ENABLE, int collision_detection_enable);

DEC_CMD_PN(ZUC_SET_SAFE_ATTITUDE, PmRpy conicalRpy; double tilt; double pan; double deviation; double deviationwarn; int openEnable; int autoEnable;
           int tipChoice);

DEC_CMD_PN(ZUC_SAFE_STAT, SafetyFuncDIMap sFuncDISettings; SafetyFuncDOMap sFuncDOSettings; SafeZone safetyZone; int safeZoneOpenEnable;
           int safeZoneAutoEnable);

DEC_CMD_PN(ZUC_SET_COLLISION_OPTION,
           int clsnDetectMethod;            // 0: based on position error, 1: momentum observation
           int clsnReactTypeWhenProgRun;    // 0: program abort and then rebound, 1: program pause without rebounding
           double clsnReactionReboudnAng;   // rebounding angle
           double clsnReactionReboudnLen;   //rebounding length
           double clsnReactionReboudnJAcc;  //rebounding joint move acc
           double clsnReactionReboudnJVel;  //rebounding joint move vel
           double clsnReactionReboudnLAcc;  //rebounding linear move acc
           double clsnReactionReboudnLVel;  //rebounding linear move vel
);

void SafetyFuncDIMap_Update(CMS* cms, SafetyFuncDIMap* x);
void SafetyFuncDOMap_Update(CMS* cms, SafetyFuncDOMap* x);